#include <avr/io.h>
#include <stdarg.h>

#include "common.h"
#include "joystick.h"

#define PORTF_ENABLE_MASK        (_BV(PORTF1) | _BV(PORTF2) | _BV(PORTF4))
#define SAMP0     (_BV(REFS0) | _BV(ADLAR) | PINF1)
#define SAMP1     (_BV(REFS0) | _BV(ADLAR) | PINF2)
#define SAMP2     (_BV(REFS0) | _BV(ADLAR) | PINF4)
#define TAKE_SAMP     (_BV(ADEN) | _BV(ADSC))
#define ADC_NOT_DONE()     (ADCSRA & _BV(ADSC))

uint8_t samples[] = {
	SAMP0,
	SAMP1,
	SAMP2
};

void joystick_init(void){
	DIDR0 = (uint8_t)~PORTF_ENABLE_MASK;
	DDRF &= (uint8_t)~PORTF_ENABLE_MASK;
	PORTF &= (uint8_t)~PORTF_ENABLE_MASK;
}

static uint8_t joystick(uint8_t sample){
    ADCH = 0x0;
	/*ADCL = 0x0; We don't use ADCL*/
    ADMUX = samples[sample];
    ADCSRA = TAKE_SAMP;
    while(ADC_NOT_DONE());
	return (uint8_t)ADCH;
}

uint8_t joystick_avg(uint8_t sample, uint8_t count){
	assert(count >= 3);
	/* Keep track of the lowest and highest result we get */
	uint8_t lowest = 0xff, highest = 0;
	uint16_t sum = 0;
	int i;
	for(i = 0; i < count; i++){
		uint8_t tmp;
		tmp = joystick(sample);
		sum += tmp;
		if(tmp < lowest){
			lowest = tmp;
		}
		if(tmp > highest){
			highest = tmp;
		}
	}
	/* Remove the lowest and highest result*/
	sum -= lowest;
	sum -= highest;

	/* Return the average of the other elements*/
	return sum / (count - 2);
}

uint8_t joystick_x(){
	return joystick_avg(0, 5);
}

uint8_t joystick_y(){
	return joystick_avg(1, 5);
}

